#include <ros/ros.h> 
#include <serial/serial.h>  //ROS已经内置了的串口包 
#include <std_msgs/Empty.h>
#include <std_msgs/String.h> 
#include <std_msgs/Header.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Int32.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/PositionTarget.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include "radio_serial/Cmd_send.h"
#include "radio_serial/Uav_state.h"
#define STATE_UAV 101
#define CMD_UAV 102
#define SELF_CMD 103
#define ARM 1
#define TAKEOFF 2
#define LAND 3
#define POS 4
#define VEL 5
#define ACC 6
#define POS_VEL 7
#define POS_ACC 8
#define VEL_ACC 9
#define POS_VEL_ACC 10
#define OPEN_TRA 11

#define LENTH_STATE_UAV 127
#define LENTH_CMD_UAV 73


uint8_t uav_num=1;

static uint8_t s_buffer[LENTH_STATE_UAV];	//分配静态存储空间
static uint8_t recv_buffer[LENTH_CMD_UAV];	//分配静态存储空间
serial::Serial ser; //声明串口对象 
float Buf2Float(unsigned char);
int check_sum(unsigned char *, int);

float Buf2Float(unsigned char *buff)
{
    float xval;
    unsigned int i;
    unsigned char *S;
    S = (unsigned char *)(&xval);
    for (i=0;i<4;i++) *(S+i) = *(buff+i);
    return xval;
}

int check_sum(unsigned char *buff, int len)
{
    int sum = 0;
    for (int i=0; i<len; i++)
    {
        sum = (sum + buff[i]) & 0xffffffff;
    }
    return sum & 0xffffffff;
}

//回调函数 
void write_callback(const radio_serial::Uav_state::ConstPtr& msg) 
{ 
      // ROS_INFO_STREAM("Writing to serial port \n" << *msg); 
      memset(s_buffer,0,sizeof(s_buffer));
      s_buffer[0] = 0xff;
      s_buffer[1] = 0xfe;
      s_buffer[2] = (msg->frame_id & 0xff);     //frame_id
      s_buffer[3] = msg->uav_index;      //uav_index
      s_buffer[4] = msg->connected;      //connected
      s_buffer[5] = msg->armed;    //armed
      s_buffer[6] = (msg->mode & 0xff);      //mode
      memcpy((s_buffer+7), &(msg->latitude), 8);     //latitude
      memcpy((s_buffer+15), &(msg->longitude), 8);    //longitude
      memcpy((s_buffer+23), &(msg->altitude), 8);     //altitude
      memcpy((s_buffer+31), &(msg->rel_altitude), 8);  //rel_altitude
      memcpy((s_buffer+39), &(msg->deg_compass), 8); //deg_compass
      memcpy((s_buffer+47), &(msg->local_x), 8);     //local_x
      memcpy((s_buffer+55), &(msg->local_y), 8);     //local_y
      memcpy((s_buffer+63), &(msg->local_z), 8);     //local_z
      memcpy((s_buffer+71), &(msg->local_lin_x), 8); //local_lin_x
      memcpy((s_buffer+79), &(msg->local_lin_y), 8); //local_lin_y
      memcpy((s_buffer+87), &(msg->local_lin_z), 8); //local_lin_z
      memcpy((s_buffer+95), &(msg->ground_speed), 8); //ground_speed
      memcpy((s_buffer+103), &(msg->air_speed), 8); //air_speed
      
      time_t tt = time(NULL);
      tm* t = localtime(&tt);
      int hour = t->tm_hour;
      int minute = t->tm_min;
      int second = t->tm_sec;
      memcpy((s_buffer+111), &hour, 4);
      memcpy((s_buffer+115), &minute, 4);
      memcpy((s_buffer+119), &second, 4);
      // std::cout << "hour=" << hour << std::endl;
      // std::cout << "minute=" << minute << std::endl;
      // std::cout << "second=" << second << std::endl;
      
      int sum_end = check_sum(s_buffer, LENTH_STATE_UAV-4);
      // std::cout << "sum_end=" << sum_end << std::endl;

      memcpy((s_buffer+LENTH_STATE_UAV-4), &sum_end, 4);
      ser.write(s_buffer, LENTH_STATE_UAV);   //发送串口数据

}
int main (int argc, char** argv) 
{ 
      //初始化节点 
      ros::init(argc, argv, "serial_port_node"); 
      //声明节点句柄 
      ros::NodeHandle nh; 
      //订阅主题，并配置回调函数
      ros::Subscriber write_sub = nh.subscribe("/outdoor/uav/state", 1, write_callback); 
      //发布主题
      ros::Publisher read_pub = nh.advertise<radio_serial::Cmd_send>("/outdoor/uav/cmd", 1); 
      try 
      { 
            //设置串口属性，并打开串口 
            ser.setPort("/dev/ttyUSB0"); 
            ser.setBaudrate(57600); 
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
      } 
      catch (serial::IOException& e) 
      { 
            ROS_ERROR_STREAM("Unable to open port "); 
            return -1; 
      } 
      //检测串口是否已经打开，并给出提示信息 
      if(ser.isOpen()) 
      { 
            ROS_INFO_STREAM("Serial Port initialized"); 
      } 
      else 
      { 
            return -1; 
      } 
      //指定循环的频率
      ros::Rate loop_rate(50);
      while(ros::ok()) 
      { 
            if(ser.available()){ 
                  // ROS_INFO_STREAM("Reading from serial port\n"); 
                  // radio_serial::Cmd_send result;
                  radio_serial::Cmd_send *result = (radio_serial::Cmd_send *)malloc(LENTH_CMD_UAV);
                  memset(recv_buffer,0,sizeof(recv_buffer)); 
                  std::vector<uint8_t> buffer_;
                  //size_t n = ser.available();
                  buffer_.reserve(LENTH_CMD_UAV+LENTH_CMD_UAV);
                  size_t n_read = ser.read(buffer_, LENTH_CMD_UAV+LENTH_CMD_UAV);
                  // ROS_INFO_STREAM("n_read.size() : " << n_read); 
 
                  for (int i=0; i<n_read-1; i++)
                  {
                        if (buffer_[0]==0xff && buffer_[1]==0xfe)
                        {
                            if (buffer_.size()>=LENTH_CMD_UAV)
                            {
                                  buffer_.erase(buffer_.begin()+LENTH_CMD_UAV, buffer_.end());
                                  for(int j=0; j<buffer_.size(); j++) {recv_buffer[j]=buffer_[j];}
                                  //for(int m=0;m<sizeof(recv_buffer);m++){printf("%02X", recv_buffer[m]);}
                                  int sum_end = check_sum(recv_buffer, LENTH_CMD_UAV-4);
                                  int real_end = 0;
                                  memcpy(&real_end, (recv_buffer+LENTH_CMD_UAV-4), 4);
                                  if (sum_end==real_end)
                                  {
                                    result->frame_id = (uint8_t)recv_buffer[2];
                                    result->uav_index = (uint8_t)recv_buffer[3];
                                    result->cmd_type = (uint8_t)recv_buffer[4];
                                    memcpy(&(result->exp_local_x), (recv_buffer+5), 8); 
                                    memcpy(&(result->exp_local_y), (recv_buffer+13), 8); 
                                    memcpy(&(result->exp_local_z), (recv_buffer+21), 8); 
                                    memcpy(&(result->exp_local_vx), (recv_buffer+29), 8); 
                                    memcpy(&(result->exp_local_vy), (recv_buffer+37), 8); 
                                    memcpy(&(result->exp_local_vz), (recv_buffer+45), 8); 
                                    //   memcpy(&(result->exp_local_ax), (recv_buffer+53), 8); 
                                    //   memcpy(&(result->exp_local_ay), (recv_buffer+61), 8); 
                                    //   memcpy(&(result->exp_local_az), (recv_buffer+69), 8); 
                                    memcpy(&(result->exp_yaw), (recv_buffer+53), 4);
                                    memcpy(&(result->hour), (recv_buffer+57), 4); 
                                    memcpy(&(result->minutes), (recv_buffer+61), 4); 
                                    memcpy(&(result->seconds), (recv_buffer+65), 4); 
				            // if ((result->frame_id==CMD_UAV or result->frame_id==SELF_CMD) and result->uav_index == uav_num){read_pub.publish(*result);}

                                    time_t t_now = time(NULL);
                                    tm* t = localtime(&t_now);
                                    int hour = t->tm_hour;
                                    int minutes = t->tm_min;
                                    int seconds = t->tm_sec;
                                    std::cout << "hour=" << hour << std::endl;
                                    std::cout << "minutes=" << minutes << std::endl;
                                    std::cout << "seconds=" << seconds << std::endl;

                                    std_msgs::Int32 time_delay; // 计算时延
                                    if (result->seconds <= seconds)
                                          time_delay.data = abs(seconds - result->seconds);
                                    else
                                          time_delay.data = 60 - abs(seconds - result->seconds);

                                    std::cout << "time_delay=" << time_delay << std::endl;

                                    read_pub.publish(*result);
                                  }
                            }
                            break;
                        }
                        else {buffer_.erase(buffer_.begin());}
                  }
                  buffer_.clear();
                  ROS_INFO_STREAM("Read: \n" << *result);
                  free(result);
            }
            //处理ROS的信息，比如订阅消息,并调用回调函数 
            ros::spinOnce(); 
            loop_rate.sleep(); 
      }
}
